/*
 * PWM.c
 *
 * Created: 27-02-2012 15:54:05
 *  Author: Bjarke
 */ 
#include <avr/io.h>


int stall = 4262;
int mult = 23;

void initPWM()
{
	//Set PORTD as output, D.4 = OCR1B, D.5 = OCR1A
	DDRD = 0xFF;
	PORTD = 0xFF;
	
	// Mode = 3 (PWM, Phase & frequency Correct, ICR1 as top)
	// Set OC1A & OC1B on match up counting / Clear OC1A & OC1B on match down counting
	// Clock prescaler = 8
	TCCR1A = 0b11110000;
	TCCR1B = 0b00010010;
		
	//Se documentation for the rest of these values
	ICR1 = 4608;
	// Duty cycle = 7.5% = stall
	OCR1A = stall;
	OCR1B = stall;	
}

void stopRobot()
{
	OCR1A = stall;
	OCR1B = stall;			
}

void startRobot(char direction, int speed)
{
	if(speed > 0 && speed <= 5)
	{
		switch (direction)
		{
			case 'F':
				OCR1A = stall-mult*speed;
				OCR1B = stall+mult*speed;
			break;
			case 'B':
				OCR1A = stall+mult*speed;
				OCR1B = stall-mult*speed;
			break;
			default: 
				turnRobot(direction,speed);
			break;
		}	
	}
	else
	{
		stopRobot();
	}		
		
}

void turnRobot(char direction, int speed)
{
	if(speed > 0 && speed <= 5)
	{
		switch (direction)
		{
			case 'R':
				OCR1A = stall+mult*speed;
				OCR1B = stall+mult*speed;
			break;
			case 'L':
				OCR1A = stall-mult*speed;
				OCR1B = stall-mult*speed;
			break;
		}	
	}
	else
	{
		stopRobot();
	}
			
}